#include "Remote.h"
#include "SBUS.h"
#include "Chassis.h"

#define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))

#define MAX_OFFSET 0.1 // 遥感偏移多少后才会移动

#define CHANNEL_VY sbus_channels[0]
#define CHANNEL_VX sbus_channels[1]
#define CHANNEL_NONE sbus_channels[2]
#define CHANNEL_VW sbus_channels[3]
#define CHANNEL_CTRLMODE sbus_channels[4]
#define CHANNEL_5 sbus_channels[5]
#define CHANNEL_6 sbus_channels[6]
#define CHANNEL_RESET sbus_channels[7]

void Remote_UART_Callback()
{
    if (CHANNEL_CTRLMODE > 600) /* 自动模式 */
    {
        HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET);
    }
    else /* 遥控模式 */
    {
        HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin, GPIO_PIN_SET);

        /******************************** 设置底盘速度 **********************************/
        float offsetVx = (sbus_channels[0] - 1024) / 783.0f;
        float offsetVy = (sbus_channels[1] - 1024) / 783.0f;
        float offsetVw = (sbus_channels[3] - 1024) / 783.0f;

        if (offsetVx < MAX_OFFSET && offsetVx > -MAX_OFFSET)
        {
            offsetVx = 0;
        }
        if (offsetVy < MAX_OFFSET && offsetVy > -MAX_OFFSET)
        {
            offsetVy = 0;
        }
        if (offsetVw < MAX_OFFSET && offsetVw > -MAX_OFFSET)
        {
            offsetVw = 0;
        }

        Chassis_SetVel(offsetVx * MAX_VX, offsetVy * MAX_VY, offsetVw * MAX_VW);
    }

    /************************************ 复位 *************************************/
    static uint8_t isReset;
    /* 拉杆4下拉 */
    if (sbus_channels[7] > 600)
    {
        isReset = 1;
    }
    else
    {
        if (isReset)
        {
            HAL_NVIC_SystemReset(); // 下拉后上拉复位
        }
    }
}

inline void Remote_Init(void)
{
    SBUS_Init(Remote_UART_Callback);
}
